#include "Stdafx.h"
#include "trackingobjects.h"

void cvLib::TrackingObjects::setupImages(int frameWidth,int frameHeight){
	image = cvCreateImage(cvSize(frameWidth,frameHeight),IPL_DEPTH_8U,3);
	hsv = cvCreateImage(cvSize(frameWidth,frameHeight),IPL_DEPTH_8U,3);
	hue = cvCreateImage(cvSize(frameWidth,frameHeight),IPL_DEPTH_8U,1);
	mask = cvCreateImage(cvSize(frameWidth,frameHeight),IPL_DEPTH_8U,1);
	backproject = cvCreateImage(cvSize(frameWidth,frameHeight),IPL_DEPTH_8U,1);
	int hdims = 16; float hranges_arr[] = {0,180}; float *hranges = hranges_arr;
	hist = cvCreateHist(1, &hdims, CV_HIST_ARRAY, &hranges, 1);
}
void cvLib::TrackingObjects::copyImage(IplImage *srcImg){
	cvCopyImage(srcImg,image);
}
void cvLib::TrackingObjects::setTrackWindow(CvRect tmpRect){
	selection = tmpRect;
}
void cvLib::TrackingObjects::convertToHSV(){
	cvCvtColor(image,hsv,CV_BGR2HSV);
}
void cvLib::TrackingObjects::startTrackObject(struct camera_parameters cp,struct calibration_constants cc){
	//cvInRangeS( hsv, cvScalar(0,smin,MIN(vmin,vmax),0),cvScalar(180,256,MAX(vmin,vmax),0), mask );
	//cvInRangeS( hsv, cvScalar(0,50,0),cvScalar(100,160,248), mask ); // one robot is ok but multi-robot not
	cvInRangeS( hsv, cvScalar(10,110,70),cvScalar(99,210,150), mask );
	// 10,256,30
	cvSplit( hsv, hue, 0, 0, 0 );
	if(!isTrackingInitialized){ // if tracking is not inilialized..
		float max_val = 0.f;
		cvSetImageROI(hue,selection);
		cvSetImageROI(mask,selection);
		cvCalcHist(&hue,hist,0,mask);
		cvGetMinMaxHistValue( hist, 0, &max_val, 0, 0 );
		cvConvertScale( hist->bins, hist->bins, max_val ? 255. / max_val : 0., 0 );
		cvResetImageROI( hue );
		cvResetImageROI( mask );
		trackWindow = selection;
		//trackObject = true;
		isTrackingInitialized = true;
	}
	cvCalcBackProject( &hue, backproject, hist );
	//cvShowImage("luoyan",hsv);
	cvAnd( backproject, mask, backproject, 0 );
	//cvShowImage("luoyan",backproject);

	cvCamShift( backproject, trackWindow,cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 20, 1 ),&trackComp,0);
	//int centroidb_x = trackWindow.x + trackWindow.width / 2 ;
	//int centroidb_y = trackWindow.x + trackWindow.height / 2 ;
	//int centroid_x = trackComp.rect.x + trackComp.rect.width / 2 ;
	//int centroid_y = trackComp.rect.x + trackComp.rect.height / 2 ;
	//x = centroid_x , y = centroid_y;
	//if (abs(centroidb_x - centroid_x) > 2 && abs(centroidb_y - centroid_y) > 2)
	//{
	//	//double tanv = (centroid_y - centroidb_y)* 1.0 / (centroid_x - centroidb_x);
	//	direction = atan2((centroid_y - centroidb_y)*1.0,(centroid_x - centroidb_x)*1.0)* 180 / PI;
	//}
	double wx1,wy1,wx2,wy2;
	double centroidb_x = trackWindow.x*1.0 + trackWindow.width*1.0 / 2 ;
	double centroidb_y = trackWindow.y*1.0 + trackWindow.height*1.0 / 2 ;
	image_coord_to_world_coord(centroidb_x, centroidb_y, 0, wx1, wy1,cp,cc);
	double centroid_x = trackComp.rect.x*1.0 + trackComp.rect.width*1.0 / 2 ;
	double centroid_y = trackComp.rect.y*1.0 + trackComp.rect.height*1.0 / 2 ;
	image_coord_to_world_coord(centroid_x, centroid_y, 0, wx2, wy2,cp,cc);
	x = wx2 , y = wy2;
	if (abs(wx2 - wx1) > 0.01 && abs(wy2 - wy1) > 0.01)
	{
		//double tanv = (centroid_y - centroidb_y)* 1.0 / (centroid_x - centroidb_x);
		direction = atan2(wy2 - wy1,wx2 - wx1)* 180 / PI;
	}

	trackWindow = trackComp.rect;
	// find the trackWindow of the next possible location of the required object..
	/*
	CvPoint p1, p2;
	p1.x = trackWindow.x;
	p2.x = trackWindow.x + trackWindow.width;
	p1.y = trackWindow.y;
	p2.y = trackWindow.y + trackWindow.height;
	cvRectangle(image,p1,p2,CV_RGB(255,0,0),1,CV_AA,0);
	*/
}
IplImage* cvLib::TrackingObjects::getTrackedFrame(){
	return image;
}
CvRect cvLib::TrackingObjects::getTrackWindow(){
	return trackWindow;
}
void cvLib::TrackingObjects::drawRectangle(CvRect objectLocation){
	CvPoint p1, p2;
	p1.x = objectLocation.x;
	p2.x = objectLocation.x + objectLocation.width;
	p1.y = objectLocation.y;
	p2.y = objectLocation.y + objectLocation.height;
	//cvRectangle(image,p1,p2,CV_RGB(255,0,0),1,CV_AA,0);
	cvRectangle(image,p1,p2,CV_RGB(0x00,0xc2,0xff),1,CV_AA,0);
}
void cvLib::TrackingObjects::drawDirection(){
	char temp[256];
	sprintf(temp,"(x=%.2lf,y=%.2lf,angle=%.2lf)",x,y,direction);
	CvFont font;
	cvInitFont(&font, CV_FONT_HERSHEY_DUPLEX , 2, 0.5);
	//cvPutText(image,temp, Point(30,30),FONT_HERSHEY_COMPLEX_SMALL,0.5, cvScalar(255, 255, 255, 0),0.1,CV_AA);
	cvPutText(image, temp, cvPoint(30,30), &font, cvScalar(0x00,0x00,0xff));
}